﻿Imports System.Math
Imports System.Drawing

Public Class CoordTrans
    ''' <summary>
    ''' 把当前点P'的坐标转为原坐标系的坐标P
    ''' </summary>
    ''' <param name="X">新坐标系的X坐标</param>
    ''' <param name="Y">新坐标系的Y坐标</param>
    ''' <param name="Angel">新坐标系的角度</param>
    ''' <param name="P1">当前点的坐标</param>
    ''' <returns>返回原坐标系的坐标</returns>
    ''' <remarks></remarks>
    Public Shared Function P1toP_Transform(ByVal X As Single, ByVal Y As Single, ByVal Angel As Single _
                         , ByVal P1 As Drawing.PointF) As Drawing.PointF
        Dim P As Drawing.PointF
        Dim g As Single = X
        Dim h As Single = Y
        P.X = P1.X * Cos(Angel * PI / 180.0) - P1.Y * Sin(Angel * PI / 180.0) + g
        P.Y = P1.X * Sin(Angel * PI / 180.0) + P1.Y * Cos(Angel * PI / 180.0) + h
        Return P
    End Function

    ''' <summary>
    ''' 把当前点P的坐标转为新坐标系的坐标P'
    ''' </summary>
    ''' <param name="X">新坐标系的X坐标</param>
    ''' <param name="Y">新坐标系的Y坐标</param>
    ''' <param name="Angel">新坐标系的角度</param>
    ''' <param name="P">当前点的坐标</param>
    ''' <returns>返回新坐标系的点坐标</returns>
    ''' <remarks></remarks>
    Public Shared Function PtoP1_Transform(ByVal X As Single, ByVal Y As Single, ByVal Angel As Single _
                         , ByVal P As Drawing.PointF) As Drawing.PointF
        Dim P1 As Drawing.PointF
        Dim g As Single = X
        Dim h As Single = Y
        P1.X = (P.X - g) * Cos(Angel * PI / 180.0) + (P.Y - h) * Sin(Angel * PI / 180.0)
        P1.Y = (P.Y - h) * Cos(Angel * PI / 180.0) - (P.X - g) * Sin(Angel * PI / 180.0)
        Return P1
    End Function


    ''' <summary>
    ''' 计算某一点绕另一点逆时针旋转后的坐标
    ''' </summary>
    ''' <param name="pt1">需要旋转的的坐标</param>
    ''' <param name="Angel">逆时针的旋转角度</param>
    ''' <param name="ptCente">旋转中心坐标</param>
    ''' <returns>返回旋转后的坐标</returns>
    ''' <remarks></remarks>
    Public Shared Function P_Rotate(ByVal pt1 As PointF, ByVal ptCente As Drawing.PointF _
                        , ByVal Angel As Single) As Drawing.PointF
        Dim P As Drawing.PointF
        Dim Rad As Single = AngleToRad(Angel)
        P.X = ptCente.X + (pt1.X - ptCente.X) * Cos(Rad) - (pt1.Y - ptCente.Y) * Sin(Rad)
        P.Y = ptCente.Y + (pt1.Y - ptCente.Y) * Cos(Rad) + (pt1.X - ptCente.X) * Sin(Rad)
        Return P
    End Function

    ''' <summary>
    ''' 角度转弧度
    ''' </summary>
    ''' <param name="Angle">角度</param>
    ''' <returns>弧度</returns>
    ''' <remarks></remarks>
    Public Shared Function AngleToRad(ByRef Angle As Single) As Single
        Return Angle * PI / 180
    End Function

    ''' <summary>
    ''' 弧度转角度
    ''' </summary>
    ''' <param name="Radian">弧度</param>
    ''' <returns>角度</returns>
    ''' <remarks></remarks>
    Public Shared Function RadToAngle(ByRef Radian As Single) As Single
        Return Radian * 180 / PI
    End Function


    ''' <summary>
    ''' 计算两点间距离
    ''' </summary>
    ''' <param name="P1"></param>
    ''' <param name="P2"></param>
    ''' <returns></returns>
    ''' <remarks></remarks>
    Public Shared Function PointLegth(ByVal P1 As PointF, ByVal P2 As PointF) As Single
        Return Math.Sqrt(Math.Pow((P1.X - P2.X), 2) + Math.Pow((P1.Y - P2.Y), 2))
    End Function


    ''' <summary>
    ''' 2个点的中心
    ''' </summary>
    ''' <param name="P1">点1</param>
    ''' <param name="P2">点2</param>
    ''' <returns>计算后的中心点坐标</returns>
    ''' <remarks></remarks>
    Public Shared Function CenterOfTwoPoint(ByVal P1 As PointF, ByVal P2 As PointF) As PointF
        Dim pt As PointF
        pt.X = (P1.X + P2.X) / 2
        pt.Y = (P1.Y + P2.Y) / 2
        Return pt
    End Function

    Public Shared Function PointAdd(ByVal P1 As PointF, ByVal P2 As PointF) As PointF
        Dim pt As PointF
        pt.X = (P1.X + P2.X)
        pt.Y = (P1.Y + P2.Y)
        Return pt
    End Function

    ''' <summary>
    ''' 2个点的差值（= P1-P2）
    ''' </summary>
    ''' <param name="P1">点1</param>
    ''' <param name="P2">点2</param>
    ''' <returns>P1-P2</returns>
    ''' <remarks></remarks>
    Public Shared Function PointSub(ByVal P1 As PointF, ByVal P2 As PointF) As PointF
        Dim pt As PointF
        pt.X = (P1.X - P2.X)
        pt.Y = (P1.Y - P2.Y)
        Return pt
    End Function



    '*函数作用： 已知2个坐标点，求从 0------->x 逆时针需旋转多少角度到该位置 
    '* 
    '*                   | 
    '*                   | 
    '*                   | 
    '*                   | 
    '*------------------------------------> x 
    '*                   | 0 
    '*                   | 
    '*                   | 
    '*                   | 
    '*                   v 
    '*                   y 
    '* 
    '*函数参数： 
    '*CvPoint2D32f pointO  - 起点 
    '*CvPoint2D32f pointA  - 终点 
    ''' <summary>
    ''' 两点计算角度
    ''' </summary>
    ''' <param name="pointO"></param>
    ''' <param name="pointA"></param>
    ''' <returns>向量OA，从 0------->x 逆时针需旋转多少角度到该位置</returns>
    ''' <remarks>函数作用： 已知2个坐标点，求从 0------->x 逆时针需旋转多少角度到该位置</remarks>
    Public Shared Function get_point_angle(ByVal pointO As PointF, ByVal pointA As PointF) As Single
        Dim angle As Single = 0
        Dim point As PointF
        Dim temp As Single

        point = New PointF((pointA.X - pointO.X), (pointA.Y - pointO.Y))

        If ((point.X = 0) And (point.Y = 0)) Then
            Return 0
        End If

        If ((point.X = 0) And (point.Y > 0)) Then
            angle = 90
            Return angle
        End If

        If ((point.X = 0) And (point.Y < 0)) Then
            angle = 270
            Return angle
        End If

        If ((point.X > 0) And (point.Y = 0)) Then
            angle = 0
            Return angle
        End If

        If ((point.X < 0) And (point.Y = 0)) Then
            angle = 180
            Return angle
        End If

        temp = Math.Abs(point.Y / point.X)
        temp = Math.Atan(temp)
        temp = temp * 180 / Math.PI


        If ((point.X > 0) And (point.Y > 0)) Then
            angle = temp
            Return angle
        End If

        If ((point.X < 0) And (point.Y > 0)) Then
            angle = 180 - temp
            Return angle
        End If

        If ((point.X < 0) And (point.Y < 0)) Then
            angle = 180 + temp
            Return angle
        End If

        If ((point.X > 0) And (point.Y < 0)) Then
            angle = 360 - temp
            Return angle
        End If

        MsgBox("getAngle error!")
        Return -1

    End Function


    '#################################################################################################################
    '#################################################################################################################
    '#################################################################################################################


    ''' <summary>
    ''' 得到一个二维数组的行数和列数
    ''' </summary>
    ''' <param name="Arrary">数组</param>
    ''' <param name="Row">行数</param>
    ''' <param name="Col">列数</param>
    ''' <remarks></remarks>
    Private Sub Find_R_C(ByVal Arrary(,) As Double, ByRef Row As Integer, ByRef Col As Integer)
        Row = Arrary.GetUpperBound(0)
        Col = Arrary.GetUpperBound(1)
    End Sub


    ''' <summary>
    ''' 
    ''' </summary>
    ''' <param name="OriginalCameraX"></param>
    ''' <param name="OriginalCameraY"></param>
    ''' <param name="OriginalCameraAngle"></param>
    ''' <param name="TemplateX"></param>
    ''' <param name="TemplateY"></param>
    ''' <param name="RobotDefaultX"></param>
    ''' <param name="RobotDefaultY"></param>
    ''' <param name="RobotDefaultAngle"></param>
    ''' <param name="CalibrationData"></param>
    ''' <param name="OutputX"></param>
    ''' <param name="OutputY"></param>
    ''' <param name="OutputAngle"></param>
    ''' <returns></returns>
    ''' <remarks></remarks>
    Public Function PointCalculationWityCameraAngle(ByVal OriginalCameraX As Double, ByVal OriginalCameraY As Double, ByVal OriginalCameraAngle As Double, _
                                                              ByVal TemplateX As Double, ByVal TemplateY As Double, _
                                                              ByVal RobotDefaultX As Double, ByVal RobotDefaultY As Double, ByVal RobotDefaultAngle As Double, _
                                                              ByVal CalibrationData() As Double, ByRef OutputX As Double, ByRef OutputY As Double, ByRef OutputAngle As Double) As Integer
        Call PointTransfer(OriginalCameraX, OriginalCameraY, CalibrationData, OutputX, OutputY)
        If OriginalCameraAngle > 180 Then OriginalCameraAngle = OriginalCameraAngle - 360
        Dim Rot As Double = OriginalCameraAngle / 180 * Math.PI
        Dim Dx As Double = 0
        Dim Dy As Double = 0
        Dx = RobotDefaultX - TemplateX
        Dy = RobotDefaultY - TemplateY
        OutputX = (OutputX + Dx * Cos(Rot)) - (Dy * Sin(Rot))
        OutputY = (OutputY + Dx * Sin(Rot)) + (Dy * Cos(Rot))
        OutputAngle = OriginalCameraAngle + RobotDefaultAngle
        Return 0
    End Function


    ''' <summary>
    ''' 点位转换
    ''' </summary>
    ''' <param name="CameraX">相机像素点X</param>
    ''' <param name="CameraY">相机像素点Y</param>
    ''' <param name="CalibrationData">标定数据</param>
    ''' <param name="OutputX">机械手坐标点X</param>
    ''' <param name="OutPutY">机械手坐标点Y</param>
    ''' <returns></returns>
    ''' <remarks>把相机的像素点转换为机械手的实际坐标</remarks>
    Public Function PointTransfer(ByVal CameraX As Double, ByVal CameraY As Double, ByVal CalibrationData() As Double, ByRef OutputX As Double, ByRef OutPutY As Double) As Integer
        Dim OriData As New VBMatrix(2, 4)
        OriData(0, 0) = 1
        OriData(0, 1) = 0
        OriData(0, 2) = CameraX
        OriData(0, 3) = -CameraY

        OriData(1, 0) = 0
        OriData(1, 1) = 1
        OriData(1, 2) = CameraY
        OriData(1, 3) = CameraX
        Dim a As Double = CalibrationData(2) / 3600 / 180 * PI
        Dim objPas As New VBMatrix(4, 1)
        objPas(0, 0) = CalibrationData(0)
        objPas(1, 0) = CalibrationData(1)
        objPas(2, 0) = CalibrationData(3) * Cos(a)
        objPas(3, 0) = CalibrationData(3) * Sin(a)
        Try
            Dim newmat As VBMatrix = OriData * objPas
            OutPutY = newmat(0, 0)  '-- Convert Y to X to meet robot requirement
            OutputX = newmat(1, 0) '-- Convert X to Y to meet robot requirement
        Catch ex As Exception
            Return 1
        End Try

        Return 0
    End Function



    ''' <summary>
    ''' 相机与机械手进行标定
    ''' </summary>
    ''' <param name="CameraPoints">相机坐标点</param>
    ''' <param name="RobotPoints">机械手坐标点</param>
    ''' <param name="dX"></param>
    ''' <param name="dY"></param>
    ''' <param name="rZ"></param>
    ''' <param name="m"></param>
    ''' <returns></returns>
    ''' <remarks></remarks>
    Public Function Calibration(ByVal CameraPoints(,) As Double, ByVal RobotPoints(,) As Double, ByRef dX As Double, ByRef dY As Double, ByRef rZ As Double, ByRef m As Double) As Integer
        Dim ErrCode As Integer = 0
        If CameraPoints.Length < 3 Or CameraPoints.Length > 12 Then
            ErrCode = 1
            Return ErrCode
            Exit Function
        End If

        Dim Row As Integer
        Dim Col As Integer
        Find_R_C(CameraPoints, Row, Col)
        Dim oriData As New VBMatrix((Row + 1) * 2, 4)

        Dim k As Integer = 0

        For i As Integer = 0 To Row
            oriData(k, 0) = 1
            oriData(k, 1) = 0
            oriData(k, 2) = CameraPoints(i, 0)
            oriData(k, 3) = -CameraPoints(i, 1)

            oriData(k + 1, 0) = 0
            oriData(k + 1, 1) = 1
            oriData(k + 1, 2) = CameraPoints(i, 1)
            oriData(k + 1, 3) = CameraPoints(i, 0)
            k = k + 2
        Next
        Find_R_C(RobotPoints, Row, Col)
        Dim TarData As New VBMatrix((Row + 1) * 2, 1)
        k = 0
        For i = 0 To Row
            TarData(k, 0) = RobotPoints(i, 0) ' sNew(i).X
            TarData(k + 1, 0) = RobotPoints(i, 1) 'sNew(i).Y
            k = k + 2
        Next

        Dim aryTmp As VBMatrix = oriData.Trans
        Dim aryTmp1 As VBMatrix = aryTmp * oriData
        aryTmp1 = aryTmp1.Inv(aryTmp1)
        aryTmp1 = aryTmp1 * aryTmp
        Dim aryOut As VBMatrix = aryTmp1 * TarData

        If aryOut.Row = 4 Then
            dX = aryOut(0, 0)
            dY = aryOut(1, 0)
            rZ = aryOut(2, 0)
            m = aryOut(3, 0)

            rZ = Math.Atan2(m, rZ)
            If rZ < 0 Then
                rZ = rZ + 2 * PI
            End If
            m = m / Sin(rZ)
            rZ = rZ * 180 / PI * 3600
        End If
        Return ErrCode
    End Function

    '/ <summary>
    '/ 拟合圆
    '/ </summary>
    Public Structure Circle
        Public X '圆心X As Double
        Public Y '圆心Y As Double
        Public R '半径R As Double
    End Structure


    '/ <summary>
    '/ 拟合圆程序
    '/ </summary>
    '/ <param name="pPointList">要拟合点集</param>
    '/ <returns>返回圆对象</returns>
    Public Shared Function FittingCircle(ByVal pPointList As List(Of PointF)) As Circle
        Dim pCircle As Circle = New Circle()
        If pPointList.Count < 3 Then
            MsgBox("最少需要三个点进行拟合")
            Return pCircle
        End If
        Dim X1 As Double = 0
        Dim Y1 As Double = 0
        Dim X2 As Double = 0
        Dim Y2 As Double = 0
        Dim X3 As Double = 0
        Dim Y3 As Double = 0
        Dim X1Y1 As Double = 0
        Dim X1Y2 As Double = 0
        Dim X2Y1 As Double = 0
        Dim i As Integer
        For i = 0 To pPointList.Count - 1 Step i + 1
            X1 = X1 + pPointList(i).X
            Y1 = Y1 + pPointList(i).Y
            X2 = X2 + pPointList(i).X * pPointList(i).X
            Y2 = Y2 + pPointList(i).Y * pPointList(i).Y
            X3 = X3 + pPointList(i).X * pPointList(i).X * pPointList(i).X
            Y3 = Y3 + pPointList(i).Y * pPointList(i).Y * pPointList(i).Y
            X1Y1 = X1Y1 + pPointList(i).X * pPointList(i).Y
            X1Y2 = X1Y2 + pPointList(i).X * pPointList(i).Y * pPointList(i).Y
            X2Y1 = X2Y1 + pPointList(i).X * pPointList(i).X * pPointList(i).Y
        Next
        Dim C As Double, D As Double, E As Double, G As Double, H As Double, N As Double
        Dim a As Double, b As Double, c1 As Double
        N = pPointList.Count
        C = N * X2 - X1 * X1
        D = N * X1Y1 - X1 * Y1
        E = N * X3 + N * X1Y2 - (X2 + Y2) * X1
        G = N * Y2 - Y1 * Y1
        H = N * X2Y1 + N * Y3 - (X2 + Y2) * Y1
        a = (H * D - E * G) / (C * G - D * D)
        b = (H * C - E * D) / (D * D - G * C)
        c1 = -(a * X1 + b * Y1 + X2 + Y2) / N
        pCircle.X = a / (-2)
        pCircle.Y = b / (-2)
        pCircle.R = Math.Sqrt(a * a + b * b - 4 * c1) / 2
        Return pCircle
    End Function
End Class